#include <Stepper.h>
#include <Servo.h>

Servo servo_lift;
Servo servo_tilt;
Servo servo_rotation;

const int lift_control_pin = 8;
int lift_control = 0;
int up_down = 1;

const int servo_tilt_pot_pin = 5;
const int servo_tilt_control_pin = 10;
int servo_rotation_pot_val;
int servo_tilt_control;

const int servo_rotation_pot_pin = 4;
const int servo_rotation_control_pin = 9;
int servo_tilt_pot_val;
int servo_rotation_control;

int val;
int temp;

int avgT; //added by dan
int avgB; //added by dan

int comp_rot = 50;
int comp_tilt = 35;

int pos_rotation = 90;
int pos_rotationLimitHigh = 180;
int pos_rotationLimitLow = 0;

int pos_tilt = 90;
int pos_tiltLimitHigh = 135;
int pos_tiltLimitLow = 45;

// analog pin numbers for reading from light sensors
int ldrlt = 0; //LDR top left
int ldrrt = 1; //LDR top rigt
int ldrld = 2; //LDR down left
int ldrrd = 3; //ldr down rigt

int TL_val;
int TR_val;
int BR_val;
int BL_val; 



void Deploy_Retract()
{ 
  int lift_flg;
  
//clockwise deploys
  if(up_down == 0)
  {
    for(int i = 0; i < 15; i++)
    {
        servo_lift.write(90);
        delay(100);
        servo_lift.write(85);
        delay(1320);
    }
    lift_flg = 1;
  }

//counter clockwise
  else if(up_down == 1)
  {
    for(int i = 0; i < 15; i++)
    {
        servo_lift.write(90);
        delay(100);
        servo_lift.write(95);
        delay(1320);
    }
    lift_flg = 0;
  }

  lift_control = 0;
  servo_lift.write(90);

  up_down = lift_flg;
  
  if(lift_flg = 0)
  {
    lift_flg = 1;
  }
  
  else
  {
    lift_flg = 0;
  }
  
}


void manualOverride()
{
  servo_tilt_control = digitalRead(servo_tilt_control_pin);
  servo_rotation_control = digitalRead(servo_rotation_control_pin);
  lift_control = digitalRead(lift_control_pin);
  
  if(servo_rotation_control == 1 && servo_tilt_control == 0)
  {
        val = analogRead(servo_rotation_pot_pin);    // reads the value of the potentiometer (value between 0 and 1023)
        val = map(val, 0, 1023, 0, 180);             // scale it to use it with the servo (value between 0 and 180)
        servo_rotation.write(val);                   // sets the servo position according to the scaled value
        pos_rotation = val;
        delayMicroseconds(15000); 
        servo_rotation_control = digitalRead(servo_rotation_control_pin);
  }
  
  else if(servo_tilt_control == 1 && servo_rotation_control == 0)
  {
        val = analogRead(servo_tilt_pot_pin);        // reads the value of the potentiometer (value between 0 and 1023)
        val = map(val, 0, 1023, 0, 180);             // scale it to use it with the servo (value between 0 and 180)
        servo_tilt.write(val);                       // sets the servo position according to the scaled value
        pos_tilt = val;
        delayMicroseconds(15000); 
        servo_tilt_control = digitalRead(servo_tilt_control_pin);
  }
   
}

void setup() 
{
  attachInterrupt(digitalPinToInterrupt(2), manualOverride, HIGH);
  servo_tilt.attach(4); 
  servo_rotation.attach(5);
  servo_tilt.write(90);
  servo_rotation.write(90);
  servo_lift.attach(6);
  servo_lift.write(90);

  delay(1000);
}

void loop() 
{
  servo_tilt_control = digitalRead(servo_tilt_control_pin);
  servo_rotation_control = digitalRead(servo_rotation_control_pin);
  lift_control = digitalRead(lift_control_pin);

int lt = analogRead(ldrrt); // top left
int rt = analogRead(ldrrt); // top right
int ld = analogRead(ldrld); // down left
int rd = analogRead(ldrrd); // down right

int tol = 25;

int dtime = 50;

int avt = (lt + rt) / 2; // average value top
int avd = (ld + rd) / 2; // average value down
int avl = (lt + ld) / 2; // average value left
int avr = (rt + rd) / 2; // average value right
int dvert = avt - avd; // check the diffirence of up and down
int dhoriz = avl - avr;// check the diffirence og left and rigt

if(lift_control == 1)
{
  Deploy_Retract();
}

if (-1*tol > dvert || dvert > tol) // check if the diffirence is in the tolerance else change vertical angle
{
    if (avt > avd)
    {
      pos_tilt = --pos_tilt;
      if (pos_tilt > pos_tiltLimitHigh)
      {
        pos_tilt = pos_tiltLimitHigh;
      }
    }

    else if (avt < avd)
    {
      pos_tilt= ++pos_tilt;
      if (pos_tilt < pos_tiltLimitLow)
      {
        pos_tilt = pos_tiltLimitLow;
      }
    }
    servo_tilt.write(pos_tilt);

}

if (-1*tol > dhoriz || dhoriz > tol) // check if the diffirence is in the tolerance else change horizontal angle
{
  if (avl > avr)
  {
    if(pos_tilt > 90)
    {
      pos_rotation = ++pos_rotation;
    }
    else if(pos_tilt <= 90)
    {
      pos_rotation = --pos_rotation;
    }
    
    if (pos_rotation < pos_rotationLimitLow)
    {
      pos_rotation = pos_rotationLimitLow;
    }
    
    else if (pos_rotation > pos_rotationLimitHigh)
    {
      pos_rotation = pos_rotationLimitHigh;
    }
  }

  else if (avl < avr)
  {
    if(pos_tilt > 90)
    {
      pos_rotation = --pos_rotation;
    }
    else if(pos_tilt <= 90)
    {
      pos_rotation = ++pos_rotation;
    }
    
    if (pos_rotation < pos_rotationLimitLow)
    {
      pos_rotation = pos_rotationLimitLow;
    }
    
    else if (pos_rotation > pos_rotationLimitHigh)
    {
      pos_rotation = pos_rotationLimitHigh;
    }
  }

  else if (avl = avr)
  {
    
  }

  servo_rotation.write(pos_rotation);

}

delay(100);
}
